"""
KWP 2000 Transport Protocol
This library provide functions to use the KWP Transport protocol for sending and receiveing requests.
"""
__author__      = ["Michael Kunkelmann","Michael J. Hohmann"]
__copyright__   = "Copyright 2017, Linde Material Handling GmbH"
__version__     = "0.0.1"
__email__       = ["michael.kunkelmann","michael.hohmann@lind-mh.de"]
__status__      = "Prototype"
__date__        = "2017-12-18"

import threading
import time
import stdtype

import sys

is_py2 = sys.version[0] == '2'
if is_py2:
    import Queue 
else:
    import queue as Queue


def security_key_12( seed ):
    A=0x974C58AB
    B=0xFEDCBA98
    C=0x98765432
    max=0xFFFFFFFF
    
    x=seed+A
    y=max-A
    
    if y < seed:
        x^=B
    else:
        x^=C
    
    low = x & 0x7ff
    upper = low << 21
    
    x=x>>11;
    x  |= upper 
    return x
    
def security_key_34( seed ):
    return seed+0x1AF7

def security_key_56( seed ):
   
   rv = 0
   
   data = {}
   data[3]= seed & 0xFF
   seed >>= 8
   data[2]= seed & 0xFF
   seed >>= 8;
   data[1]= seed & 0xFF
   seed >>= 8;
   data[0]= seed & 0xFF

   data[0] += 0x97
   data[1] += 0x28
   data[2] += 0xAA
   data[3] += 0x55
   
   data[0] &= 0xFF
   data[1] &= 0xFF
   data[2] &= 0xFF
   data[3] &= 0xFF

   rv = data[3]
   rv <<= 8
   rv |= data[2]
   rv <<= 8
   rv |= data[1]
   rv <<= 8
   rv |= data[0]

   return rv;
    
    
def packitem( format,value ):
    signdness=format[:1]
    length=int(format[1:-1])
    endian=format[-1:]
    rv=[]

    if signdness=='S' and value<0:
        value+=1<<length
    
    length = length/8 # in byte
    while length > 0:
        rv.append( value&0xff)
        value>>=8
        length-=1
        
    if endian=='B':
        rv.reverse()
        
    return rv


class sid:
    StartDiagnosticSession          = 0x10
    EcuReset                        = 0x11
    ReadDiagnosticTroubleCodes      = 0x13
    ClearDiagnosticTroubleCodes      = 0x14
    ReadStatusOfDiagnosticTroubleCodes = 0x17 
    ReadEcuIdentification           = 0x1a
    StopDiagnosticSession           = 0x20
    ReadDataByLocalIdentifier       = 0x21
    ReadDataByCommonIdentifier      = 0x22
    SecurityAccess                  = 0x27
    StartRoutineByLocalIdentifier   = 0x31
    RequestDownload                 = 0x34
    TransferData                    = 0x36
    RequestTransferExit             = 0x37
    WriteDataByLocalIdentifier      = 0x3b
    WriteDataByCommonIdentifier     = 0x2E
    TesterPresent                   = 0x3E
    ReadMemoryByAddress             = 0x40 #0x23
    WriteMemoryByAddress            = 0x3d
    
class response_code:
    GENERALREJECT                   = 0x010
    SERVICENOTSUPPORTED             = 0x011
    SUBFUNCTIONNOTSUPPORTED         = 0x012
    INVALIDFORMAT                   = 0x012
    BUSYREPEATREQUEST               = 0x021
    CONDITIONSNOTCORRECT            = 0x022
    REQUESTSEQUENCEERROR            = 0x022
    ROUTINENOTCOMPLETED             = 0x023
    REQUESTOUTOFRANGE               = 0x031
    RESPONSEPENDING                 = 0x078
    SERVICENOTSUPPORTEDINACTIVEMODE = 0x080
    ACCESSDENIED                    = 0x033
    INVALIDKEY                      = 0x035
    EXCEEDNUMOFATTEMPTS             = 0x036
    TIMEDELAYNOTEXPIRED             = 0x037
    DOWNLOADNOTACCEPTED             = 0x040
    IMPROPERDOWNLOADTYPE            = 0x041
    CANNTDOWNLOADTOADDRESS          = 0x042
    CANNTDOWNLOADNUMOFBYTES         = 0x043
    UPLOADNOTACCEPTED               = 0x050
    IMPROPERUPLOADTYPE              = 0x051
    CANNTUPLOADFROMADDRESS          = 0x052
    CANNTUPLOADNUMOFBYTES           = 0x053
    TRANSFERSUSPENDED               = 0x071
    TRANSFERABORTED                 = 0x072
    ILLEGALADDRESSINBLOCKTRANSFER   = 0x074
    ILLEGALBYTECOUNTINBLOCKTRANSFER = 0x075
    ILLEGALBLOCKTRANSFERTYPE        = 0x076
    BLOCKTRANSFERDATACHECKSUMERROR  = 0x077
    INCORRECTBYTECNTDGBLOCKTRANSFER = 0x079
    INVALIDCHECKSUM                 = 0x0FB
    
    POSITIVE_RESPONSE               = 0x100 # From here on there are no really KWP response codes
    NEGATIVE_RESPONSE               = 0x101
    TIMED_OUT                       = 0x102  
    PENDING                         = 0x103
    UNKNOWN_RESPONSE                = 0x104



"""
class kwp_request:
    def __init__(self,sid,data,callback,timeout,repeat):
        self.sid=sid
        self.data=data
        self.callback=callback
        self.timeout=timeout
        self.repeat=repeat
        self.status=None
        self.finished=False
        self.response=None


    def blocking(self,timeout=0.010):
        while self.finished == False:
            time.sleep(timeout)
        return self

    def is_posive_response(self):
        return self.status==POSITIVE_RESPONSE

    def unpack( format ):
        return stdtype.unpack( format, self.data[2:] )
"""

class request:
    def __init__(self, data ,callback=None,callback_data=None,timeout=0.050, repeat=0):
        self.data=data
        self.sid=self.data[0]                
        self.callback=callback
        self.callback_data=callback_data
        self.timeout=timeout
        self.repeat=repeat
        self.status=None
        self.finished=False

        self.response=[]

    def blocking(self,timeout=0.010):
        while self.finished == False:
            time.sleep(timeout)
        return self

    def is_posive_response(self):
        return self.status==response_code.POSITIVE_RESPONSE

    def unpack( self, format ):
        return stdtype.unpack( format, self.response )


class kwp_tp:
    def __init__(self, cancard_obj, rx_id, tx_id, debug = 0 ):
        self.cancard_obj = cancard_obj
        
        self.rx_id = rx_id
        self.tx_id = tx_id

        self.timeout = 0.1
        self.debug = debug
              
    def process(self,msg):
        count=0
        msg.status=response_code.PENDING
        #msg.timeout=self.timeout
        if msg.timeout==None:
            msg.timeout=self.timeout
        while (count <= msg.repeat) and (msg.status != response_code.POSITIVE_RESPONSE ):
            count+=1
            self.transmit(msg)
        if msg.callback is not None:
            msg.callback(msg.callback_data)
        msg.finished=True


    def single_frame(self, msg):
            # slingle frame
            data=[len(msg.data)]  # lenght and sid
            data.extend(msg.data)           #add data
            while len(data)<8: # blow data to the lenth of 8 its maybe requested by the kwp counterpart
                data.append(0)
            status = self.cancard_obj.send_msg( data, self.tx_id, self.debug )
            self.fetch_response(msg)

    """    def pending(self,msg):
       status, can_msg = cancard_obj.get_msg()
       if status == 0:
          while can_msg.tagData.msg.data[:4]==[0x03,0x7f,msg.sid,0x78]:
              status, can_msg = self.cancard_obj.get_msg()
       return status, can_msg
"""
    def fetch_response(self, msg):
            total=0
            timeout=time.time() + msg.timeout       
            while time.time() < timeout:
                status, can_msg = self.cancard_obj.get_msg()        
                if status == 0 and can_msg.tagData.msg.id == self.rx_id:

                    # Build Debug Output
                    if self.debug:
                       msg_debug_out = ""
                       for i in range(0, len(can_msg.tagData.msg.data)):          
                          msg_debug_out += "%02X " % can_msg.tagData.msg.data[i]
                       #print ("RX <- " + msg_debug_out)
                    
                    if (can_msg.tagData.msg.data[0] & 0x10) == 0x10: # receive first frame of an extendet message 
                        timeout=time.time() + msg.timeout
                        msg.response=[]
                        msg.response.extend(can_msg.tagData.msg.data[2:])
                        total=can_msg.tagData.msg.data[0]&0x0F
                        total <<= 8
                        total += can_msg.tagData.msg.data[1]
                        total -= 6                       
                        status = self.cancard_obj.send_msg( [0x30,0x00,0x01,0x00,0x00,0x00,0x00,0x00], self.tx_id )
                    else:
                        if can_msg.tagData.msg.data[0] >> 4 == 2:                                                                                                            
                            timeout=time.time() + msg.timeout
                            if total > 7:
                                msg.response.extend(can_msg.tagData.msg.data[1:]) #
                                total -= 7
                            else:
                                msg.response.extend(can_msg.tagData.msg.data[1:total+1]) #
                                total=0
                                msg.status = response_code.POSITIVE_RESPONSE
                                return                                                                 
                        else:              
                           if can_msg.tagData.msg.data[1] == msg.sid+0x40: #positive response                             
                               msg.status = response_code.POSITIVE_RESPONSE
                               msglen=can_msg.tagData.msg.data[0]                               
                               msg.response = can_msg.tagData.msg.data[1:msglen+1]                               
                               return 
                           else:
                               if can_msg.tagData.msg.data[1] == 0x7f: #negative response or pending
                                   if can_msg.tagData.msg.data[:4]==[0x03,0x7f,msg.sid,0x78]:
                                       print("# message pending - reset timer ")
                                       timeout=time.time() + msg.timeout
                                   else:
                                       msg.status = can_msg.tagData.msg.data[3]
                                       msglen=can_msg.tagData.msg.data[0]
                                       #msg.response.extend(can_msg.tagData.msg.data[1:msglen+1]) #
                                       msg.response = can_msg.tagData.msg.data[1:msglen+1]
                                       
                                       return
                               else:
                                   msg.status = response_code.UNKNOWN_RESPONSE

            msg.status=response_code.TIMED_OUT

    def transmit(self, msg):
        if len(msg.data) > 7:
            self.send_segmented(msg)
        else:
            self.single_frame(msg)

    def wait_for_flow_control(self,timeout):
        # time.sleep(timeout)   #comment this in KCPC ;uncomment this in k-module
        status, msg = self.cancard_obj.get_msg()

    def send_segmented(self, msg):
        #msg>x
        #msg.data<4096
        """ First Frame
            0x1F  or first 4 bits of size
            0xFF  last 8 bit of size
            SID
            data[5]  first 5 byte of data
            """
        size=len( msg.data )
        data=[ 0x10 | (size>>8), size & 0xFF ,  msg.data.pop(0), msg.data.pop(0), msg.data.pop(0), msg.data.pop(0), msg.data.pop(0), msg.data.pop(0) ]
        
        status = self.cancard_obj.send_msg( data, self.tx_id )

        # print "Wait Flow-Controle"
        #self.wait_for_flow_control(msg.timeout)
        self.wait_for_flow_control(msg.timeout)

        frame_counter=1
        while(len(msg.data)!=0):
            data=[0x20| (frame_counter&0xF)]

            for index in xrange(0,7):
                if len(msg.data)==0:
                    data.append(0x00) # fill with zeros
                else:
                    data.append(msg.data.pop(0))
                index+=1
            frame_counter+=1
            status = self.cancard_obj.send_msg( data, self.tx_id )
        self.fetch_response(msg)



class kwp2k:
    def __init__( self, cancard_obj, debug, rx_id, tx_id , rx_timeout = 0.500 ):
        self.cancard = cancard_obj
        self.kwp_tp = kwp_tp(cancard_obj, rx_id, tx_id, debug )
        self.request_queue=Queue.Queue(0)
        self.rx_timeout=rx_timeout
        self.active=True   #
        self.tx_thread = threading.Thread(name="kwp", target = self.kwp_thread_function )
        self.tx_thread.setDaemon(True)
        self.tx_thread.start()

    def kwp_thread_function(self):
        while self.active:
            self.kwp_tp.process(self.request_queue.get() )    

    def transmit(self, data, callback=None, callback_data=None, timeout=0.5, repeat=0 ):        
        msg=stdtype.pack_to_list(data)     
        obj=request( msg, callback, callback_data, timeout, repeat )
        self.request_queue.put( obj )
        return obj        
